//
// Created by lijia on 2021/1/12.
//

#ifndef PERISCOPE_MPU_H
#define PERISCOPE_MPU_H

#include <stdint.h>
#include "stm32f1xx_hal.h"

#define MPU_I2C_ADDR 0x68
#define MAG_I2C_ADDR 0x0c
#define BARO_I2C_ADDR 0x76

#define REG_WHO_AM_I 0x75
#define VALUE_WHO_AM_I 0x71

#define REG_USER 0x6a
#define VALUE_USER_ENABLE_I2C 0x20

#define REG_POWER 0x6b
#define VALUE_POWER_RESET 0x80
#define VALUE_POWER_OFF 0x00
#define VALUE_POWER_ON 0x03

#define REG_POWER_2 0x6c
#define VALUE_POWER_2_ENABLE_ALL 0x00

#define REG_GYRO_BIAS 0x13

#define REG_CONFIG 0x1a
#define VALUE_CONFIG_USE_FIFO 0x40
#define VALUE_CONFIG_LPF_BW_41Hz 0x03
#define VALUE_CONFIG_LPF_BW_BASED 0x04
#define VALUE_CONFIG_LPF_BW_25Hz 0x05

#define REG_GYRO_CONFIG 0x1b
#define VALUE_GYRO_CONFIG_USE_LPF 0x11
#define VALUE_GYRO_CONFIG_BASE 24
#define VALUE_GYRO_CONFIG_USE_HP_ADVICE 0x18

#define REG_ACC_CONFIG 0x1c
#define VALUE_ACC_CONFIG_2G 0x00u
#define VALUE_ACC_CONFIG_4G 0x08u
#define VALUE_ACC_CONFIG_8G 0x10u

#define REG_SAMPLE_RATE 0x19
#define VALUE_SAMPLE_RATE_NONE 0
#define VALUE_SAMPLE_RATE_50Hz 20

#define REG_CTRL 0x6a
#define VALUE_CTRL_ENABLE_FIFO 0x40

#define REG_MASTER_I2C_CTRL 0x24

#define REG_SLAVE0_ADDR 0x25
#define VALUE_SLAVE0_ADDR 0x8c

#define REG_SLAVE0_REGISTER 0x26
#define VALUE_SLAVE0_REGISTER 0x03

#define REG_SLAVE0_CTRL 0x27

#define REG_I2C_PASSTHROUGH 0x37
#define VALUE_I2C_PASSTHROUGH 0x02

#define REG_SLAVE_DATA 0x49
#define REG_SLAVE0_OUT_DATA 0x63

#define REG_MAG_CTRL 0x0a
#define VALUE_MAG_ENABLE 0x11

#define REG_GYRO_DATA 0x43
#define REG_ACC_DATA 0x3b
#define REG_MAG_DATA 0x03

#define REG_MAG_ID 0x00
#define VALUE_MAG_ID 0x48

#define REG_MAG_CTRL 0x0a
#define REG_MAG_ASA_X 0x10
#define REG_MAG_ASA_Y 0x11
#define REG_MAG_ASA_Z 0x12
#define REG_MAG_CTRL_2 0x0b
#define REG_MAG_STATE 0x02
#define REG_MAG_STATE_2 0x09

#define REG_BARO_T1 0x88
#define REG_BARO_T2 0x8a
#define REG_BARO_T3 0x8c
#define REG_BARO_P1 0x8e
#define REG_BARO_P2 0x90
#define REG_BARO_P3 0x92
#define REG_BARO_P4 0x94
#define REG_BARO_P5 0x96
#define REG_BARO_P6 0x98
#define REG_BARO_P7 0x9a
#define REG_BARO_P8 0x9c
#define REG_BARO_P9 0x9e

#define REG_BARO_WHO_AM_I 0xd0
#define VALUE_BARO_WHO_AM_I 0x58

#define REG_BARO_RESET 0xe0
#define VALUE_BARO_RESET 0xb6

#define REG_BARO_CALIBRATION 0xe1

#define REG_BARO_CONTROL 0xf4
#define VALUE_BARO_CONTROL 0x57

#define REG_BARO_CONFIG 0xf5
#define VALUE_BARO_CONFIG 0xbc

#define REG_BARO_PRESSURE 0xf7

#define REG_BARO_TEMPERATURE 0xfa

#define X_AXIS 0
#define Y_AXIS 1
#define Z_AXIS 2

#define MAG_X_AXIS 1
#define MAG_Y_AXIS 0
#define MAG_Z_AXIS 2

typedef struct {
	int16_t gyro[3];
	int16_t acceleration[3];
	int16_t magnetic[3];
	uint32_t lastUpdateTime;
} MpuData;

typedef struct {
	int16_t acceleration[3];
	int16_t gyro[3];
	float magneticScaling[3];
	float magneticInclination;
	int16_t magneticLowerBound[3];
	int16_t magneticUpperBound[3];
	int16_t magneticBias[3];
	int16_t magneticNorm[3];
} MpuCalibrationData;

typedef struct {
	float pressure;
	float temperature;
	uint32_t lastUpdateTime;
} BarometerData;

typedef struct {
	uint16_t t1;
	int16_t t2;
	int16_t t3;
	uint16_t p1;
	int16_t p2;
	int16_t p3;
	int16_t p4;
	int16_t p5;
	int16_t p6;
	int16_t p7;
	int16_t p8;
	int16_t p9;
} BarometerCalibration;

extern void mpu_init(I2C_HandleTypeDef *);

extern void mpu_tick();

extern const MpuData *mpu_get();

extern const BarometerData *baro_get();

extern const MpuCalibrationData *mpu_calibration_get();

#endif //PERISCOPE_MPU_H
